/**
  ****************************(C) COPYRIGHT 2024 Polarbear****************************
  * @file       mechanical_arm.c/h
  * @brief      机械臂功能。
  * @note
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.1     Apr-21-2024     Penguin         1. done
  *
  @verbatim
  ==============================================================================

  ==============================================================================
  @endverbatim
  ****************************(C) COPYRIGHT 2024 Polarbear****************************
  */

#include "mechanical_arm.hpp"
#include "motor.hpp"

mechanical_state_e mechanical_arm_state;
 void MechanicalArmInit()
 {
/**减速比与can属性初始化 */
motors[0].gearratio_and_can_init(1, 1, 1, 1);
motors[1].gearratio_and_can_init(1, 1, 1, 1);
motors[2].gearratio_and_can_init(1, 1, 1, 1);
motors[3].gearratio_and_can_init(1, 1, 1, 1);
motors[4].gearratio_and_can_init(1, 1, 1, 1);
motors[5].gearratio_and_can_init(1, 1, 1, 1);
/**限幅初始化 */
motors[0].limit_init(0,0,0,0);
motors[1].limit_init(0,0,0,0);
motors[2].limit_init(0,0,0,0);
motors[3].limit_init(0,0,0,0);
motors[4].limit_init(0,0,0,0);
motors[5].limit_init(0,0,0,0);
/*PID初始化*/
motors[0].pid_init(0.01,0.01,0.01);
motors[1].pid_init(0.01,0.01,0.01);
motors[2].pid_init(0.01,0.01,0.01);
motors[3].pid_init(0.01,0.01,0.01);
motors[4].pid_init(0.01,0.01,0.01);
motors[5].pid_init(0.01,0.01,0.01);
/**输入端口连接 */










}

void state_cycling()
{
switch(mechanical_arm_state)
{
case CALIBRATION_TO_ZERO:
...
break;

case MAP:

for(const auto& joint:joints)
{
joint::expect_pos=//通过数据输入端口获取实时数据
joint::sensor_data_update();
if(joint::error_flag==1)
{
  mechanical_arm_state=PROGRAM_ERROR;
  return;
}
joint::caculate_and_output();
}
break;

case AUTO:
.....

case PROGRAM_ERROR:
error_stop();
break;  




}




}



void TIM3_IRQHandler()
{







  
}


 





